import javax.media.j3d.*;
import javax.vecmath.*;

import tracker.TrackerThread.SimpleSensor;

/**
 * The primary hand behaviour class controls the location of the hand geometry, provides the 
 * flying around the world ability and allows us to drag objects at around the world.
 */
public abstract class HandBehaviorGeneric extends Behavior {

	/*Wake up conditions*/
    protected WakeupOnElapsedFrames nextFrame;
    protected WakeupOnCollisionEntry collision;
    protected WakeupOnCollisionExit collisionExit;
    protected WakeupOnCollisionMovement collisionMovement;
    protected WakeupOr wakeupConditions;

    protected HandProxy hand;					/* Geometry of the hand */
	
	/*Sensors*/
    protected SimpleSensor handTrackerSensor;			/* Location sensor for the hand */
    protected ButtonDevice buttonDevice;		/* retrieves controller buttons state */

	/*Transformations*/
    protected TransformGroup viewPlatform;		/* Used to store the view transform (where we are looking) */
    protected Transform3D handTransform;		/* Transform matrix for the hand geometry*/
    protected Transform3D viewTransform;		/* The view transformation matrix info*/
    protected TransformGroup worldBranch; 		/* Transform describing the location of the world */

    protected TransformGroup draggingObject;	/* an objects transform group that is being dragged */
    protected Vector3d pickedObjectOffset;		/* Offset added relative to the hand */
    protected Matrix3d pickedObjectRotationOffset;	/* Offset added relative to the rotation of the hand */
    protected long lastTick = System.currentTimeMillis();	/* last system time in milliseconds since an update */
    protected float tickDiff;								/* time in seconds since last update */

    public boolean isSixDOF = false;				    /* can the user fly freely in 6DOF or are they restricted to the XZ plane */	
    
    protected final static float FLYING_SPEED = 
    	Float.parseFloat(VR.getDefaults().getProperty("FLYING_SPEED"));	/* user's movement speed in units per second */
    
    protected final static float LEVITATE_SPEED = 
    	Float.parseFloat(VR.getDefaults().getProperty("LEVITATE_SPEED"));	/* user's levitation speed in units per second */
    	
    
    
    public HandBehaviorGeneric(Hand hand, Sensor handSensor, ButtonDevice buttonDevice, TransformGroup viewPlatform, 
		TransformGroup worldBranch) {
    	
    	/*Assign class parameters*/
        this.hand = new HandProxy(hand);
        this.handTrackerSensor = (SimpleSensor)handSensor;
        this.buttonDevice = buttonDevice;
        this.viewPlatform = viewPlatform;
        this.worldBranch = worldBranch;
        
        /*Set up defaults and initalise*/
        viewTransform = new Transform3D();
        handTransform = new Transform3D();
        pickedObjectOffset = new Vector3d();
        pickedObjectRotationOffset = new Matrix3d();
        draggingObject = null;
        
        /* Set up the behaviour conditions */
        if (this.hand.isNull()){
        	nextFrame = new WakeupOnElapsedFrames(0);
        	wakeupConditions = new WakeupOr(new WakeupCriterion[]{nextFrame});    
        } else {
        	nextFrame = new WakeupOnElapsedFrames(0);
        	collision = new WakeupOnCollisionEntry(hand, WakeupOnCollisionEntry.USE_GEOMETRY);
            collisionExit = new WakeupOnCollisionExit(hand, WakeupOnCollisionEntry.USE_GEOMETRY);
            collisionMovement = new WakeupOnCollisionMovement(hand, WakeupOnCollisionEntry.USE_GEOMETRY);
        	wakeupConditions = new WakeupOr(new WakeupCriterion[]{nextFrame, collision, collisionExit, collisionMovement});    
        }
    }
   
    public void initialize() {
    	wakeupOn (nextFrame);
    }
    
	//******************************************************************//
	//                     AVATAR MOVEMENT CODE                         //
	//******************************************************************//
    /** called before computing movement for a particular frame.
	 * frame. This is to ensure that movement stays in sync with the
	 * units per second. */
	protected void beginFrameMovement(){
		tickDiff = (System.currentTimeMillis() - lastTick) / 1000.0f;
	}

	/** call after all movement has been computed for a particular
	 * frame. This is to ensure that movement stays in sync with the
	 * units per second. */
	protected void endFrameMovement(){
		lastTick = System.currentTimeMillis();
	}	

	private void moveViewVector(Vector3f movement){
		moveViewVector(movement, isSixDOF);
	}
	
	/** moves the world by the distance specified in the parameter vector */
	private void moveViewVector(Vector3f movement, boolean is6DOF){
		
		/*Find the view direction*/
		viewPlatform.getTransform(viewTransform);
		Vector3f viewDirection = new Vector3f(movement);
		viewTransform.transform(viewDirection);

		/*Add the offset to the worlds existing location by adding to the appropriate translation parts of the matrix.*/
		Transform3D worldTF = new Transform3D();
		worldBranch.getTransform(worldTF);
		
		Vector3d wtm = new Vector3d();
		worldTF.get(wtm);	
		wtm.x += viewDirection.x;
		wtm.z += viewDirection.z;
		if (is6DOF) /*This makes flying around the world a little easier by removing the up/down transformation*/
			wtm.y += viewDirection.y;
		
		worldTF.set(wtm);
		worldBranch.setTransform(worldTF);

   	}
		
	/** move the user's viewpoint forward.
	 * @param distance the units to move forward */
	protected void forward(float distance){
		moveViewVector(new Vector3f(0, 0, distance * tickDiff));
	}
	
	/** strafes the user's viewpoint left or right */
	protected void strafe(float distance){
		moveViewVector(new Vector3f(-distance * tickDiff, 0, 0));
	}

	/** levitates the user's viewpoint up or down */
    protected void levitate(float distance) {
    	moveViewVector(new Vector3f(0, -distance * tickDiff, 0), true);
    }
	
	//******************************************************************//
	//                    HAND and OBJECT DRAGGING                      //
	//******************************************************************//
	protected void setHandPosition() {

		/*Set the locaiton of the users hand*/
		if (handTrackerSensor != null){
			/* get the hand location from the tracker */
			handTrackerSensor.getRead(handTransform);            
	        hand.setTransform(handTransform);
		} else {
			/* else set the hand location to directly infront of the user */
			viewPlatform.getTransform(handTransform);
			Vector3f viewDir = new Vector3f(0, -0.5f, -1.75f);
			handTransform.transform(viewDir);
			handTransform.setTranslation(viewDir);
			hand.setTransform(handTransform);
		}
		
        /* If we need to move an object with the hand */
        if (draggingObject != null)
            moveDraggedObject(draggingObject, handTransform);
	}
   
	/** tests to see whether the user is attempting to grab a new object with
	 * their hand. */
	protected void testForGrab(){
		
		 // Make sure we can only drag one thing around at a time
		 if (draggingObject != null)
            return;
		 
		 // make sure the hand geometry exists
		 if (hand.isNull())
			 return;
      
		 // s there is no object to drag?
         TransformGroup group = getPickedObjectFromCollision();
         if (group == null)
            return;

		// calcuate the location offset from which we have grabbed the object. We do 
		// this in Vworld coordinates
		Transform3D groupTransform = new Transform3D();
		group.getTransform(groupTransform);
		
		Transform3D groupLocalToVWorld = new Transform3D();
		Vector3d groupVWorldLocation = new Vector3d();
		group.getLocalToVworld(groupLocalToVWorld);
		groupLocalToVWorld.mul(groupTransform);
		groupLocalToVWorld.get(groupVWorldLocation);
 	
		// work out the hand location (assumed to be in vworld coordinates 
		Transform3D handtransform_ = new Transform3D();
        Vector3d handLocation = new Vector3d();
        hand.getTarget().getTransform(handtransform_);
        handtransform_.get(handLocation);
      
        pickedObjectOffset.sub(handLocation, groupVWorldLocation);

        // convert the picked object offset from vworld coordinates back into object 
        // local transform coordinates
        groupLocalToVWorld.invert();
        groupLocalToVWorld.transform(pickedObjectOffset);
        
        draggingObject = group;
        moveDraggedObject(draggingObject, handTransform);
      
	}
   
	/** finds the TransformGroup of the object that has collided with the virtual hand */
	public TransformGroup getPickedObjectFromCollision() {
		
		SceneGraphPath draggedObject_;
		
        if (collision.hasTriggered())
            draggedObject_ = collision.getTriggeringPath();
        else if (collisionExit.hasTriggered())
        	draggedObject_ = collisionExit.getTriggeringPath();
        else if (collisionMovement.hasTriggered())
         	draggedObject_ = collisionMovement.getTriggeringPath();
        else
            return null;
      
        Node node = draggedObject_.getObject();
        if (node == null) 
            return null;
      
        Object userData = node.getUserData();
        if (userData == null) 
            return null;
      
        if (!(userData instanceof VRObject))
        	return null;
      
        ((VRObject)userData).collided();
        
        /*If we find an object that can be dragged around the world return its transform group*/
        return (TransformGroup)userData;
	}
	
	/**
	 * moves a dragged object relative to the hand movement. 
	 * This method will move an object being dragged relative to the user's
	 * hand. 
	 */
	protected void moveDraggedObject (TransformGroup object, Transform3D handTransform) {
		
        /* calculate the VWorld -> Local coordinate transformation of the object */
		Transform3D vworldToObjectLocal = VR.getVWorldToLocal(object);
		Transform3D handTransformCopy = new Transform3D(handTransform);
		vworldToObjectLocal.mul(handTransformCopy);
		
		Vector3d handPosInObjectSpace = new Vector3d();
		vworldToObjectLocal.get(handPosInObjectSpace);
		handPosInObjectSpace.sub(pickedObjectOffset);
		
		/* set the object's transform */
		Transform3D objectTransform = new Transform3D();
		object.getTransform(objectTransform);
        objectTransform.setTranslation(handPosInObjectSpace);

        object.setTransform(objectTransform);
        
        if (object instanceof VRObject)
        	((VRObject)object).dragging();
	}
	
	/**
	 *  given a point on a ray, A, and a directional vector, up, this method will
	 * return the closest point on that ray to to point P. 
	 */
	Vector3d getClosestPointOnLineSegment(Vector3d A, Vector3d up, Vector3d P){
		/* A + ((P - A).(up)) / ((up).(up)) * up */		
		Vector3d XsubP = new Vector3d();
		XsubP.sub(P, A);
		double t = XsubP.dot(up);
		
		Vector3d v2 = new Vector3d(up);
		t /= v2.dot(up);
		
		Vector3d closest = new Vector3d();
		closest.set(up);
		closest.scaleAdd(t, A);
		
		return closest;
	}
	
	Vector3d getClosestPointToObjectsAxis(TransformGroup object, Point3d P){

		Transform3D vworldToObjectLocal = VR.getVWorldToLocal(object);
		Transform3D handTransformCopy = new Transform3D(handTransform);
		handTransformCopy.mul(vworldToObjectLocal);

		Vector3d handPosInObjectSpace = new Vector3d();
		handTransformCopy.get(handPosInObjectSpace);
				
		/* grab the objects location in local coordinates */
		Vector3d objectLocation = new Vector3d();
		Transform3D objecTransform = new Transform3D();
		object.getTransform(objecTransform);
		objecTransform.get(objectLocation);

		/* grab the object's up vector */
		Vector3d objectUp = new Vector3d(0, 1, 0);
		objecTransform.transform(objectUp);
		objectUp.add(objectLocation);
		
		Vector3d closest = getClosestPointOnLineSegment(objectLocation, objectUp, handPosInObjectSpace);
		closest.sub(objectLocation);

		return closest;
	}
	
   
	/* if the user is dragging an object, drop it */
	public void testForDrop() {
		if (draggingObject != null)
			draggingObject = null;
    }
	
	/** proxy class that sends messages on to target Hand object. If the
	 * target is null, the messages are ignored. This is to keep implementations
	 * of HandBehaviorGeneric clean removing the need for following tests:
	 * 
	 * 	   if (hand != null) hand.setTopRightButtonState(true);
	 * 
	 * The above case can occur when using keyboard mode, where only one hand
	 * exists, but you still need the behaviour of both hands.
	 */
	protected class HandProxy {
		
		private Hand target = null;
		
		HandProxy(Hand target){ this.target = target; }
		
		/* proxy methods */
		void setTopRightButtonState(boolean state){  if (target != null) target.setTopRightButtonState(state); }
		void setRightButtonState(boolean state){     if (target != null) target.setRightButtonState(state); }
		void setTopLeftButtonState(boolean state){   if (target != null) target.setTopLeftButtonState(state); }
		void setLeftButtonState(boolean state){      if (target != null) target.setLeftButtonState(state); }
        void setTopMiddleButtonState(boolean state){ if (target != null) target.setTopMiddleButtonState(state); }
        void setTransform(Transform3D t3d){          if (target!=null) target.setTransform(t3d); }

        Hand getTarget(){ return target; }
        boolean isNull(){ return target == null; }
	}
}
